﻿using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace Yoozoo.Gameplay.Liberty
{
    public class VehicleSplineDebug2 : MonoBehaviour
    {
        public Rigidbody rigidbody;

        public bool pathLoop = true;
        public List<Transform> targetPoints;

        public Transform frontPoint;

        public float endDistance = 2;
        
        public float force = 1;
        public float turn_speed = 1;
        
        private int _pointIndex = 0;

        private void Update()
        {
            if (Input.GetKeyDown(KeyCode.Space))
            {
                rigidbody.AddForce(Vector3.forward * 100);
            }
        }

        private void FixedUpdate()
        {

            Transform target = targetPoints[_pointIndex];
            Vector3 frontPos = frontPoint.position;
            frontPos.y = 0;
            Vector3 tf = target.position - frontPos;

            
            
            if (tf.magnitude > endDistance)
            {
                AddForce(tf.normalized * force);
            }
            else
            {
                // 切换目标点
                if (_pointIndex + 1 < targetPoints.Count || (_pointIndex == targetPoints.Count - 1 && pathLoop))
                {
                    if (pathLoop && _pointIndex == targetPoints.Count - 1)
                    {
                        _pointIndex = 0;
                    }
                    else
                    {
                        _pointIndex++;
                    }
                    UnityEngine.Debug.LogError("切换目标点：" + _pointIndex);
                    
                    Transform target2 = targetPoints[_pointIndex];
                    Vector3 frontPos2 = frontPoint.position;
                    frontPos2.y = 0;
                    Vector3 tf2 = target2.position - frontPos2;
                    AddForce(tf2.normalized * force);
                }
                else
                {
                    //AddForce(Vector3.zero);
                }
               
            }

            Vector3 relativePos = targetPoints[_pointIndex].position - transform.position;
            relativePos.y = 0;
            Face_waypoint(relativePos);
        }
        
          void Face_waypoint(Vector3 relativePos)
          {
              Quaternion rotation = Quaternion.LookRotation(relativePos, Vector3.up);
              transform.rotation = Quaternion.Slerp(transform.rotation, rotation, Time.deltaTime * turn_speed);  // slerp ( from.rotation, to.rotation, speed)
          }
          
          public Vector3 GetPointDirection(int pointIndex)
          {
              if (pointIndex == 0)
              {
                  return (targetPoints[pointIndex + 1].position - targetPoints[pointIndex].position).normalized;
              }
              else if (pointIndex == targetPoints.Count- 1)
              {
                  return (targetPoints[pointIndex].position - targetPoints[pointIndex-1].position).normalized;
              }
              else
              {
                  Vector3 v1 = (targetPoints[pointIndex + 1].position - targetPoints[pointIndex].position).normalized;
                  Vector3 v2 = (targetPoints[pointIndex].position - targetPoints[pointIndex - 1].position).normalized;
                  return ((v1 + v2) * 0.5f).normalized;
              }
          }

          public void AddForce(Vector3 velocity)
          {
              rigidbody.AddForce(velocity,ForceMode.VelocityChange);
          }


          public void AddTorque(Vector3 velocity)
          {
              rigidbody.AddTorque(velocity,ForceMode.VelocityChange);
          }


          private void OnDrawGizmos()
          {
              Gizmos.color = Color.yellow;
              Gizmos.DrawLine(transform.position, transform.position + transform.forward * 10);

              for (int i = 0; i < targetPoints.Count; i++)
              {
                  if (i != targetPoints.Count - 1)
                  {
                      Gizmos.DrawLine(targetPoints[i].position,targetPoints[i+1].position);
                  }
              }

              if (pathLoop)
              {
                  Gizmos.DrawLine(targetPoints[0].position,targetPoints[targetPoints.Count - 1].position);
              }
            
          }
          
    }
    
    
    
}